#include "map.h"
#include <string.h>
#include <stdio.h>

// 3x3二维码网格ID分布：
// 7 8 9
// 4 5 6
// 1 2 3
// ID范围1~9，邻居没有则为0
const QRNode_t g_qr_map[9] = {
    // id, up, down, left, right
    {1, 4, 0, 0, 2},   // 1号二维码
    {2, 5, 0, 1, 3},   // 2号二维码
    {3, 6, 0, 2, 0},   // 3号二维码
    {4, 7, 1, 0, 5},   // 4号二维码
    {5, 8, 2, 4, 6},   // 5号二维码
    {6, 9, 3, 5, 0},   // 6号二维码
    {7, 0, 4, 0, 8},   // 7号二维码
    {8, 0, 5, 7, 9},   // 8号二维码
    {9, 0, 6, 8, 0}    // 9号二维码
};

// 验证二维码ID是否有效
bool IsValidQRID(int id) {
    return (id >= 1 && id <= 9);
}

// 验证两个二维码是否相邻
bool AreNeighbors(int id1, int id2) {
    if (!IsValidQRID(id1) || !IsValidQRID(id2)) return false;
    if (id1 == id2) return false;
    
    const QRNode_t* node = &g_qr_map[id1-1];
    return (node->up == id2 || node->down == id2 || 
            node->left == id2 || node->right == id2);
}

// 获取二维码的邻居数量
int GetNeighborCount(int id) {
    if (!IsValidQRID(id)) return 0;
    
    const QRNode_t* node = &g_qr_map[id-1];
    int count = 0;
    if (node->up > 0) count++;
    if (node->down > 0) count++;
    if (node->left > 0) count++;
    if (node->right > 0) count++;
    return count;
}

// 改进的路径规划：返回从start_id到end_id的下一步二维码ID（无路径返回0）
int Map_NextStepToTarget(int start_id, int end_id)
{
    // 输入验证
    if (!IsValidQRID(start_id) || !IsValidQRID(end_id)) {
        printf("路径规划错误：无效的二维码ID (start=%d, end=%d)\r\n", start_id, end_id);
        return 0;
    }
    
    if (start_id == end_id) {
        printf("路径规划：已在目标位置\r\n");
        return end_id;
    }
    
    // 检查是否直接相邻
    if (AreNeighbors(start_id, end_id)) {
        printf("路径规划：直接移动到相邻位置 %d -> %d\r\n", start_id, end_id);
        return end_id;
    }
    
    // BFS队列和前驱数组
    int queue[9], front = 0, rear = 0;
    int visited[10] = {0}; // ID 1~9
    int prev[10] = {0};    // 记录前驱
    int distance[10] = {0}; // 记录距离
    
    // 入队
    queue[rear++] = start_id;
    visited[start_id] = 1;
    distance[start_id] = 0;
    
    printf("开始BFS路径规划：%d -> %d\r\n", start_id, end_id);
    
    while (front < rear) {
        int curr = queue[front++];
        
        // 查找curr的邻居
        const QRNode_t* node = &g_qr_map[curr-1];
        int neighbors[4] = {node->up, node->down, node->left, node->right};
        
        for (int i = 0; i < 4; ++i) {
            int nb = neighbors[i];
            if (nb > 0 && !visited[nb]) {
                queue[rear++] = nb;
                visited[nb] = 1;
                prev[nb] = curr;
                distance[nb] = distance[curr] + 1;
                
                printf("发现路径：%d -> %d (距离=%d)\r\n", curr, nb, distance[nb]);
                
                if (nb == end_id) {
                    // 回溯找第一步
                    int step = end_id;
                    while (prev[step] != start_id) {
                        step = prev[step];
                    }
                    printf("路径规划成功：下一步移动到 %d (总距离=%d)\r\n", step, distance[end_id]);
                    return step;
                }
            }
        }
    }
    
    printf("路径规划失败：无法从 %d 到达 %d\r\n", start_id, end_id);
    return 0; // 无法到达
}

// 新增：获取完整路径（用于调试和验证）
int Map_GetFullPath(int start_id, int end_id, int* path, int max_path_len)
{
    if (!IsValidQRID(start_id) || !IsValidQRID(end_id) || !path || max_path_len <= 0) {
        return 0;
    }
    
    if (start_id == end_id) {
        path[0] = start_id;
        return 1;
    }
    
    // BFS队列和前驱数组
    int queue[9], front = 0, rear = 0;
    int visited[10] = {0};
    int prev[10] = {0};
    
    queue[rear++] = start_id;
    visited[start_id] = 1;
    
    while (front < rear) {
        int curr = queue[front++];
        const QRNode_t* node = &g_qr_map[curr-1];
        int neighbors[4] = {node->up, node->down, node->left, node->right};
        
        for (int i = 0; i < 4; ++i) {
            int nb = neighbors[i];
            if (nb > 0 && !visited[nb]) {
                queue[rear++] = nb;
                visited[nb] = 1;
                prev[nb] = curr;
                
                if (nb == end_id) {
                    // 回溯构建完整路径
                    int path_len = 0;
                    int step = end_id;
                    
                    // 先计算路径长度
                    while (step != start_id) {
                        path_len++;
                        step = prev[step];
                    }
                    path_len++; // 包含起始点
                    
                    if (path_len > max_path_len) {
                        printf("路径过长，超出缓冲区大小\r\n");
                        return 0;
                    }
                    
                    // 构建路径（反向）
                    step = end_id;
                    for (int j = path_len - 1; j >= 0; j--) {
                        path[j] = step;
                        step = prev[step];
                    }
                    
                    printf("完整路径：");
                    for (int j = 0; j < path_len; j++) {
                        printf("%d", path[j]);
                        if (j < path_len - 1) printf(" -> ");
                    }
                    printf(" (长度=%d)\r\n", path_len);
                    
                    return path_len;
                }
            }
        }
    }
    
    printf("无法找到从 %d 到 %d 的路径\r\n", start_id, end_id);
    return 0;
}

// 新增：路径验证函数
bool Map_ValidatePath(int* path, int path_len)
{
    if (!path || path_len <= 0) return false;
    
    for (int i = 0; i < path_len - 1; i++) {
        if (!AreNeighbors(path[i], path[i+1])) {
            printf("路径验证失败：%d 和 %d 不相邻\r\n", path[i], path[i+1]);
            return false;
        }
    }
    
    printf("路径验证成功\r\n");
    return true;
}

// 新增：获取备选路径（当主路径失败时）
int Map_GetAlternativePath(int start_id, int end_id, int failed_next_id)
{
    if (!IsValidQRID(start_id) || !IsValidQRID(end_id)) return 0;
    
    const QRNode_t* node = &g_qr_map[start_id-1];
    int neighbors[4] = {node->up, node->down, node->left, node->right};
    
    printf("寻找备选路径，避开失败的 %d\r\n", failed_next_id);
    
    for (int i = 0; i < 4; ++i) {
        int nb = neighbors[i];
        if (nb > 0 && nb != failed_next_id) {
            // 检查通过这个邻居是否能到达目标
            int temp_path[9];
            int path_len = Map_GetFullPath(nb, end_id, temp_path, 9);
            if (path_len > 0) {
                printf("找到备选路径：通过 %d\r\n", nb);
                return nb;
            }
        }
    }
    
    printf("没有找到备选路径\r\n");
    return 0;
} 